#ifndef POSEFRAME_H
#define POSEFRAME_H

#include <vector>

#include "Thirdparty/Sophus/sophus/geometry.hpp"

#include "ImuTypes.h"
//#include "KeyPoseFrame.h"
#include "Filter/WheelOdom.h"
#include "Converter.h"
#include "Settings.h"

#include <mutex>

#include "Eigen/Core"
#include "sophus/se3.hpp"

#include "nav_msgs/Odometry.h"

namespace msf
{

class KeyPoseFrame;
class ConstraintPoseImu;

class PoseFrame
{
public:
      PoseFrame();

      // 拷贝构造函数
      PoseFrame(const PoseFrame &poseframe);

      // Odom构造函数
      PoseFrame(const nav_msgs::OdometryConstPtr &odom, const double &timestamp, PoseFrame* pPrevPF = static_cast<PoseFrame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());

      // 设置odom位姿(Imu位姿不修改)
      void SetPose(const Sophus::SE3<float> &Tow);

      // 设置IMU速度
      void SetVelocity(Eigen::Vector3f Vw);

      Eigen::Vector3f GetVelocity() const;

      // 设置IMU位姿和速度(隐式改变odom位姿)
      void SetImuPoseVelocity(const Eigen::Matrix3f &Rwo, const Eigen::Vector3f &two, const Eigen::Vector3f &Vwo);

      Eigen::Matrix<float,3,1> GetImuPosition() const;
      Eigen::Matrix<float,3,3> GetImuRotation();
      Sophus::SE3<float> GetImuPose();

      void SetNewBias(const IMU::Bias &b);

      ConstraintPoseImu* mpcpi;

      bool imuIsPreintegrated();
      void setIntegrated();

      bool isSet() const;

      // 从odom位姿中计算旋转、平移和odom中心矩阵
      void UpdatePoseMatrices();
      
      // 返回odom中心
      inline Eigen::Vector3f GetCameraCenter(){
            return mOw;
      }

      // 返回旋转的逆
      inline Eigen::Matrix3f GetRotationInverse(){
            return mRwo;
      }

      inline Sophus::SE3<float> GetPose() const {
            return mTow;
      }

      inline Eigen::Matrix3f GetRwo() const {
            return mRwo;
      }

      inline Eigen::Vector3f GetOw() const {
            return mOw;
      }

      inline bool HasPose() const {
            return mbHasPose;
      }

      inline bool HasVelocity() const {
            return mbHasVelocity;
      }

private:

      Sophus::SE3<float> mTow;
      Eigen::Matrix<float,3,3> mRwo;
      Eigen::Matrix<float,3,1> mOw;
      Eigen::Matrix<float,3,3> mRow;
      Eigen::Matrix<float,3,1> mtow;
      bool mbHasPose;

      //Rcw_ not necessary as Sophus has a method for extracting the rotation matrix: Tcw_.rotationMatrix()
      //tcw_ not necessary as Sophus has a method for extracting the translation vector: Tcw_.translation()
      //Twc_ not necessary as Sophus has a method for easily computing the inverse pose: Tcw_.inverse()

      // IMU linear velocity
      Eigen::Vector3f mVw;
      bool mbHasVelocity;

public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      // 帧时间戳
      double mTimeStamp;

      IMU::Bias mPredBias;

      // IMU bias
      IMU::Bias mImuBias;

      // Imu calibration
      IMU::Calib mImuCalib;

      // 从上一关键位姿帧开始的IMU预积分
      IMU::Preintegrated* mpImuPreintegrated;
      KeyPoseFrame* mpLastKeyPoseFrame;

      // 指向先前帧的指针
      PoseFrame* mpPrevPoseFrame;
      IMU::Preintegrated* mpImuPreintegratedPoseFrame;

      // 当前和下一帧id
      static long unsigned int nNextId;
      long unsigned int mnId;

      // 参考关键位姿帧
      KeyPoseFrame* mpReferenceKPF;

      static bool mbInitialComputations;

private:

      bool mbIsSet;

      bool mbImuPreintegrated;

      std::mutex *mpMutexImu;

public:

      //save nav_Odometry &odom信息
      OdomData odom_data;

      Sophus::SE3<double> T_test;

};

}// namespace msf

#endif // FRAME_H